#include "datapool.h"
#include <math.h>

float cmd_data[4][3] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
float fb_data[4][3] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
float cmd_imu[3] = {0.0 / 180.0 * 3.14, 0.0/180.0*3.14, 0.0/180.0*3.14};
float fb_imu[3] = {0, 0, 0};

GaitParams state_gait_params[] =
    {
        //{s.h, d.a., u.a., f.p., s.l., fr., s.d.}

        {NAN, NAN, NAN, NAN, NAN, NAN, NAN}, // STOP
                                             //高，下，上，飞行相，步长，频率，步差
                                             //{0.22, 0.02, 0.04, 0.35, 0.12, 2.0, 0.0}, // TROT[bessel]
        {0.20, 0.01, 0.028, 0.35, 0.125, 2.0, 0.0}, // TROT[sin]

        {0.17, 0.04, 0.06, 0.35, 0.0, 2.0, 0.0},  // BOUND
        {0.15, 0.00, 0.06, 0.25, 0.0, 1.5, 0.0},  // WALK
        {0.12, 0.05, 0.0, 0.75, 0.0, 1.0, 0.0},   // PRONK
        {NAN, NAN, NAN, NAN, NAN, NAN, NAN},      // JUMP
        {0.15, 0.05, 0.05, 0.35, 0.0, 1.5, 0.0},  // DANCE
        {0.15, 0.05, 0.05, 0.2, 0.0, 1.0, 0.0},   // HOP
        {NAN, NAN, NAN, NAN, NAN, 1.0, NAN},      // TEST
        {NAN, NAN, NAN, NAN, NAN, NAN, NAN},      // ROTATE
        {0.15, 0.07, 0.06, 0.2, 0.0, 1.0, 0.0},   // FLIP
        {0.17, 0.04, 0.06, 0.35, 0.1, 2.0, 0.06}, // TURN_TROT
        {NAN, NAN, NAN, NAN, NAN, NAN, NAN}       // RESET
};
